﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics.Contracts;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

//closed = {}
//q = emptyqueue;
//q.enqueue(0.0, makepath(start))
//while q is not empty
//    p = q.dequeueCheapest
//    if closed contains p.last then continue;
//    if p.last == destination then return p
//    closed.add(p.last)
//    foreach n in p.last.neighbours 
//        newpath = p.continuepath(n)
//        q.enqueue(newpath.TotalCost + estimateCost(n, destination), newpath)
//return null

namespace XRoute.PathFinding
{
    /// <summary>
    /// Represents a path along a graph. Steps are held in a one-directional
    /// linked list, the head of which is the last added step.
    /// <para>The term &quot;step&quot; means &quot;the edge leading to a node and the node&quot;,
    /// so the cost of a step is the cost/length of the edge plus the cost of the node.
    /// The cost of a step may also be referred to as distance between two neighbour nodes.</para>
    /// </summary>
    /// <typeparam name="TNode">The type of the node.</typeparam>
    /// <remarks>Instances of this type are immutable.</remarks>
    public class Path<TNode> : IEnumerable<TNode>
    {
        ///////////////////////////////////////////////////////////////////////

        readonly TNode _head;
        readonly Path<TNode> _tail;
        readonly double _totalCost;


        ///////////////////////////////////////////////////////////////////////

        /// <summary>
        /// Gets the last step of the path.
        /// </summary>
        public TNode Head
        {
            get { return _head; }
        }

        /// <summary>
        /// Gets the previous steps.
        /// </summary>
        public Path<TNode> Tail
        {
            get { return _tail; }
        }

        /// <summary>
        /// Gets the total cost of the path, which is the sum of all steps
        /// of the path.
        /// </summary>
        public double TotalCost
        {
            get { return _totalCost; }
        }

        /// <summary>
        /// Finds the shortest path from the specified origin node to the
        /// specified destination node.
        /// </summary>
        /// <param name="origin">The origin.</param>
        /// <param name="destination">The destination.</param>
        /// <param name="distance">Callback invoked to calculate the distance
        /// from one node to one of its neighbour nodes.</param>
        /// <param name="estimate">Callback invoked to estimate the distance
        /// from the specified node to <paramref name="destination"/>.</param>
        /// <param name="neighbours">Callback invoked to enumerate the neighbours
        /// of the specified node.</param>
        /// <returns>The shortest path from the specified origin node to the
        /// specified destination node. <c>null</c> if there is no path leading
        /// from <paramref name="origin"/> to <paramref name="destination"/>.</returns>
        public static Path<TNode> FindPath(
            TNode origin,
            TNode destination,
            Func<Path<TNode>, TNode, double> distance,
            Func<TNode, double> estimate,
            Func<TNode, IEnumerable<TNode>> neighbours)
        {
            Contract.Requires(origin != null);
            Contract.Requires(destination != null);
            Contract.Requires(distance != null);
            Contract.Requires(estimate != null);
            Contract.Requires(neighbours != null);

            var closed = new HashSet<TNode>();
            var open = new PriorityQueue<double, Path<TNode>>();

            open.Enqueue(0, new Path<TNode>(origin));

            while (open.IsEmpty == false)
            {
                var path = open.Dequeue();

                if (closed.Contains(path.Head))
                    continue;

                if (path.Head.Equals(destination))
                    return path;

                closed.Add(path.Head);

                foreach (var node in neighbours(path.Head))
                {
                    var d = distance(path, node);
                    var newPath = path.AddStep(node, d);

                    open.Enqueue(newPath.TotalCost + estimate(node), newPath);
                }
            }

            return null;
        }

        /// <summary>
        /// Returns an enumerator that iterates through the collection.
        /// </summary>
        /// <returns>
        /// A <see cref="T:System.Collections.Generic.IEnumerator`1" /> that can be used to iterate through the collection.
        /// </returns>
        public IEnumerator<TNode> GetEnumerator()
        {
            for (var p = this; p != null; p = p.Tail)
                yield return p.Head;
        }

        /// <summary>
        /// Returns an enumerator that iterates through a collection.
        /// </summary>
        /// <returns>
        /// An <see cref="T:System.Collections.IEnumerator" /> object that can be used to iterate through the collection.
        /// </returns>
        IEnumerator IEnumerable.GetEnumerator()
        {
            return this.GetEnumerator();
        }


        ///////////////////////////////////////////////////////////////////////

        /// <summary>
        /// Initializes a new instance of the <see cref="Path{TNode}"/> class.
        /// </summary>
        /// <param name="head">The head node of the new path.</param>
        /// <remarks>Internal for access from unit tests.</remarks>
        internal Path(TNode head)
        : this(head, null, 0)
        {
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="Path{TNode}"/> class.
        /// </summary>
        /// <param name="head">The last step.</param>
        /// <param name="tail">The previous steps.</param>
        /// <param name="totalCost">The total cost of the new path.</param>
        Path(TNode head, Path<TNode> tail, double totalCost)
        {
            _head = head;
            _tail = tail;
            _totalCost = totalCost;
        }

        /// <summary>
        /// Creates a new instance of <see cref="Path{TNode}"/> that has one more
        /// step than <c>this</c> instance.
        /// </summary>
        /// <param name="step">The next step.</param>
        /// <param name="stepCost">The cost of the next step.</param>
        /// <returns>A new instance of <see cref="Path{TNode}"/> that has one more
        /// step than <c>this</c> instance.</returns>
        /// <remarks>Internal for access from unit tests.</remarks>
        internal Path<TNode> AddStep(TNode step, double stepCost)
        {
            return new Path<TNode>(step, this, TotalCost + stepCost);
        }
    }
}
